package ch.ethz.fcl.process;

import java.awt.image.BufferedImage;
import java.util.ArrayList;
import java.util.List;

import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;

import ch.ethz.fcl.Constant;
import ch.ethz.fcl.KinectContainer;
import ch.ethz.fcl.input.Coordinate;
import ch.ethz.fcl.tools.ImageTools;
import ch.ethz.fcl.tools.Tools;

public class DepthImageProcess {
	private KinectContainer container;

	private int cannyThresh = 40;
	double up = 110 * Constant.COLOR_SCALE,
			lowBound = 80 * Constant.COLOR_SCALE;

	private List<MatOfPoint> contours;
	private List<MatOfPoint> polys;

	public DepthImageProcess(KinectContainer container) {
		this.container = container;
	}

	public void process(BufferedImage image) {
		process(ImageTools.imageToMat(image));
	}

	public Mat process(Mat image) {
		image = update(image);
		return image;
	}

	public Mat inRange(Mat input) {

		Mat output = new Mat(input.rows(), input.cols(), CvType.CV_8UC1);

		input.convertTo(output, CvType.CV_8UC1, 0.00390625);

		// for (int i = 0; i < input.rows(); i++)
		// for (int j = 0; j < input.cols(); j++) {
		// double[] value = input.get(i, j);
		// if (value == null)
		// output.put(i, j, new double[] { 0 });
		// else {
		// double[] newValue = new double[3];
		// if (value[0] >= up || value[0] <= lowBound)
		// output.put(i, j, new double[] { 255 });
		// else {
		// newValue[0] = newValue[1] = newValue[2] = (value[0] - lowBound)
		// / (up - lowBound) * 255;
		// output.put(i, j, newValue);
		// }
		// }
		// }
		return output;
	}

	private Mat update(Mat image) {
		image = inRange(image);

		contours = new ArrayList<MatOfPoint>();
		polys = new ArrayList<MatOfPoint>();

		Mat canny = new Mat();
		Mat pyr = new Mat();
		Mat timg = new Mat();
		Mat gray = new Mat();
		Mat hierarchy = new Mat();

		// down-scale and upscale the image to filter out the noise
		Imgproc.pyrDown(image, pyr,
				new Size(image.cols() / 2, image.rows() / 2));
		Imgproc.pyrUp(pyr, timg, image.size());

		// if (timg.type() == CvType.CV_8UC3)
		// Imgproc.cvtColor(timg, gray, Imgproc.COLOR_RGB2GRAY);
		// else if (timg.type() == CvType.CV_8UC1)
		gray = timg;

		// apply Canny. Take the upper threshold from slider
		// and set the lower to 0 (which forces edges merging)
		Imgproc.Canny(gray, canny, 0, cannyThresh);
		// dilate canny output to remove potential
		// holes between edge segments
		Imgproc.dilate(canny, canny, new Mat());

		// find contours and store them all as a list
		Imgproc.findContours(canny, contours, hierarchy, Imgproc.RETR_LIST,
				Imgproc.CHAIN_APPROX_SIMPLE);

		contours = filterContours(contours);

		// generate polygons for each contours
		for (int i = 0; i < contours.size(); i++) {
			MatOfPoint2f curve = new MatOfPoint2f(contours.get(i).toArray());
			MatOfPoint2f approx = new MatOfPoint2f();
			Imgproc.approxPolyDP(curve, approx, 3, true);

			polys.add(new MatOfPoint(approx.toArray()));
		}

		// System.out.println("Found " + contours.size() + " contours and "
		// + polys.size() + " rectangles");

		return image;
	}

	/**
	 * filter out impossible contours
	 * 
	 * @param input
	 * @return
	 */
	private List<MatOfPoint> filterContours(List<MatOfPoint> input) {
		List<MatOfPoint> output = new ArrayList<MatOfPoint>();

		// remove small size contours
		for (int i = 0; i < input.size(); i++) {
			MatOfPoint contour = input.get(i);
			double area = Math.abs(Imgproc.contourArea(contour));
			// test the area size of the image
			if (area < Constant.MIN_CONTOUR_SIZE)
				continue;
			// test area and length ratio
			double length = Imgproc.arcLength(
					new MatOfPoint2f(contour.toArray()), true);

			if (length * length > 100 * area)
				continue;

			int outBoundPointsSize = 0;
			Point[] pts = contour.toArray();

			for (int j = 0; j < pts.length; j++) {
				if (pts[j].x < Coordinate.TABLE_OUTBOUND_MIN_X
						|| pts[j].x > Coordinate.TABLE_OUTBOUND_MAX_X)
					outBoundPointsSize++;
			}

			if (outBoundPointsSize > pts.length / 2)
				continue;

			boolean exist = false;
			for (int j = 0; j < output.size(); j++) {
				MatOfPoint c = output.get(j);
				double match = Imgproc.matchShapes(input.get(i), c,
						Imgproc.CV_CONTOURS_MATCH_I2, 0);
				double area1 = Imgproc.contourArea(c);
				double area2 = Imgproc.contourArea(input.get(i));
				double areaDiffRatio = Math.abs(area2 - area1)
						/ (area1 + area2);

				Point centroid1 = Tools.centroid(c);
				Point centroid2 = Tools.centroid(input.get(i));
				double centroidDist = Tools.dist(centroid1, centroid2);

				if (match < 2 && areaDiffRatio < 0.2 && centroidDist < 20) {
					exist = true;
					break;
				}
			}
			if (!exist)
				output.add(contour);
		}

		return output;
	}

	public List<MatOfPoint> getContours() {
		return contours;
	}

	public List<MatOfPoint> getPolys() {
		return polys;
	}

	public KinectContainer getContainer() {
		return container;
	}
}
